/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Test;

import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Robot.*;
import Robot.Sensors.*;
import Utils.*;

/**
 *
 * @author alejo
 */
public class Colciencias1 {
    
    public static void main(String args[])
    {

        boolean isSimulated = true;
        boolean showArbFrame = true;
        boolean showMapObstFrame = true;


        Robot intruso;
        Robot camara;

        RealPosition2d intrPos;
        RealPosition2d camPos;

        Sonar intrSonar;

        //Creacion del intruso y de la camara
        if(isSimulated)
        {
            intruso = new Robot("real","localhost",6665, 0);
            camara = new Robot("camara","localhost",6665,1);


            camPos = new RealPosition2d(camara,"camaraPos",0);

            intrPos = new RealPosition2d(intruso,"intrusoPos",0);
            intrSonar = new Sonar(intruso,"realIR",0);

            intruso.runThreaded(-1, -1);
            camara.runThreaded(-1, -1);

            while(!intrSonar.updatePoses());

            Pose2d initialPose;
            initialPose = new Pose2d((float)15,(float)25,-(float)Math.PI/2);
            intrPos.setPosition(initialPose);
        }
        else
        {
            camara = new Robot("camara","192.168.0.1",6665,1);
            camPos = new RealPosition2d(camara,"camaraPos",0);

            camara.runThreaded(-1, -1);
        }


        //habilitar movimiento de la camara
        camPos.setMotorPower(true);

        

        if(isSimulated)
        {
            //intrPos.setMaxSpeed(0.4f);
            //intrPos.setRotControl(0.3f,1.0f);

            camPos.setRotControl(0.3f,0.5f);
        }


        NxMSWander intrWander = new NxMSWander("intrWander");
        
        Behavior[] array = {intrWander};
        

        

        //VectorAdditionArbitrator intrArb = new VectorAdditionArbitrator(array,intrPos,showArbFrame);
        //VectorAdditionArbitrator camArb = new VectorAdditionArbitrator(array,camPos,showArbFrame);
        
        //intrArb.enabled = true;
        //camArb.enabled = true;

        

        while(true)
        {
         
            //intrArb.step();
            //camArb.step();


            try
            {
                Thread.sleep(50);
            }
            catch(Exception e)
            {}
        }
        
        /*
        
      
        while(true)
        {

            float[] result = null;
            while(result == null)
                result = ir.read(Sensor.READ_MODE_WAIT);


            
            int resultadoIR = 0;
            for(int i=0; i<result.length;i++)
            {
                if(result[i] == 0)
                {
                    switch(i)
                    {
                        case 0:
                            resultadoIR += 1;
                            break;
                        case 1:
                            resultadoIR += 2;
                            break;
                        case 2:
                            resultadoIR += 4;
                            break;          
                        case 3:
                            resultadoIR += 8;
                            break;          
                        case 4:
                            resultadoIR += 16;
                            break;           
                        case 5:
                            resultadoIR += 32;
                            break;                            
                    }
                }
                
                
            }
            
            System.out.println(resultadoIR);
            
            
            switch(resultadoIR)
            {
                case 1:
                case 2:
                case 3:
                case 6:
                case 7:
                case 15:
                case 31:
                    posReal.setSpeed(0, -0.8f);
                    break;
                
                case 4:
                case 8:
                case 12:
                case 30:
                
                    posReal.setSpeed(-0.3f,0.8f);
                    break;
                case 16:
                case 32:
                case 48:
                case 24:
                case 56:
                case 28:
                case 60:
                case 62:
                    posReal.setSpeed(0, 0.8f);
                    break;
                default:
                    posReal.setSpeed(0.3f, 0);
            }
        }*/
     
    }

}
